#pragma once
#include "common.h"

namespace Horizon{

class ExtendedKalmanFilter{
public:
    ExtendedKalmanFilter(){
        is_initialized_ = false;
    }
    ~ExtendedKalmanFilter(){}

    void Initialization(Eigen::VectorXd x_in){
        x_ = x_in;
    }

    void SetF(Eigen::MatrixXd F_in){
        F_ = F_in;
    }

    void SetP(Eigen::MatrixXd P_in){
        P_ = P_in;
    }

    void SetQ(Eigen::MatrixXd Q_in){
        Q_ = Q_in;
    }

    void Prediction(){
        x_ = F_ * x_;
        Eigen::MatrixXd Ft = F_.transpose();
        P_ = F_ * P_ * Ft + Q_;
    }

    void CalculateJacobBianMatrix(){
        MatrixXd Hj(3,4);
        // 得到状态参数
        float px = x_(0);
        float py = x_(1);
        float vx = x_(2);
        float vy = x_(3);
        // 预计算一个设置的组避免重复计算
        float c1 = px * px + py * py;
        float c2 = sqrt(c1);
        float c3 = c1 * c2;
        
        if(fabs(c1) < 0.0001){
            H_ = Hj;
            return ;
        }
        // 计算雅可比矩阵
        Hj << (px/c2),                  (py/c2),                    0 ,     0,
              -(py/c1),                 (px/c1),                    0 ,     0,
              py*(vx*py - vy*px)/c3,     px*(px*vy -py*vx)/c3,      px/c2,      py/c2;
        return ;
    }

    void MeasurementUpdate(const Eigen::VectorXd &z){
        double rho = sqrt(x_(0)*x_(0) + x_(1)*x_(1));
        double theta = atan2(x_(1),x_(0));
        double rho_dot = (x_(0)*x_(2) + x_(1)*x_(3))/rho;
        VectorXd h = VectorXd(3);
        h << rho, theta, rho_dot;

        VectorXd y = z - h;

        CalculateJacobBianMatrix();

        MatrixXd S = H_ * P_ * H_.transpose() + R_;
        MatrixXd K = P_ * H_.transpose() * S.inverse();
        x_ = x_ + (K * y);
        MatrixXd I = MatrixXd::Identity(x_.size(),x_.size());
        P_ = MatrixXd(I - K * H_) * P_;
    
    }

private:
    // 初始化的标志
    bool is_initialized_;

    // 状态向量
    Eigen::VectorXd x_;

    // 状态转移矩阵
    Eigen::MatrixXd F_;

    // 状态协方差矩阵
    Eigen::MatrixXd P_;

    // 过程噪声
    Eigen::MatrixXd Q_;

    // 测量矩阵
    Eigen::MatrixXd H_;

    // 测量噪声
    Eigen::MatrixXd R_;

};



}